#ifndef NAVI_H
#define NAVI_H


#include "Action.h"
#include "pid.h"
#include "ahrs.h"
#include "bmi088.h"
#include "map.h"
#include "abs.h"





typedef struct{
	float pos_x;
	float pos_y;
	float last_pos_x;
  float	last_pos_y;
}chassis_position_t;
typedef enum{
	YES,
	NO,
	NEXT1,
	NEXT2
}is_cpt_e;
typedef struct{
   pid_param_t pid_PosSpeed_x_param;                 
   pid_param_t pid_PosSpeed_y_param;                
   pid_param_t pid_pos_x_out_param;                   
   pid_param_t pid_pos_x_inner_param;                 
   pid_param_t pid_pos_y_out_param;                   
   pid_param_t pid_pos_y_inner_param;                 
   pid_param_t pid_OutAngle_param;                    
   pid_param_t pid_InnerAngle_param;                   
   pid_param_t path_speed_pid_param;                  

}ops_pid_param_t;

typedef struct {
	ops_pid_param_t ops_pid;
	point_t *path;
	int8_t path_num;
}ops_param_t;

/**
 * @brief 
 *
*/
typedef struct {
	Action_POS_t *chassis_pos;          
	fp32 vx;																
	fp32 vy;																
	fp32 Last_X;															
	fp32 Last_Y;															 
	fp32 current_x;														 	
	fp32 current_y;														

  const point_t *chassis_map;		
  const ops_param_t *param;	
	PathState_t state;
	
	 AHRS_Eulr_t ops_imu_pos;
   AHRS_Gyro_t ops_gyro;

	 struct 
	 {
		fp32 vx;
		fp32 vy;
		fp32 yaw_angle;
	 }final_out;
	 
	
	pid_type_def pid_PosSpeed_x;              
	pid_type_def pid_PosSpeed_y;	         
	pid_type_def pid_pos_x_out;
	pid_type_def pid_pos_x_inner;
	pid_type_def pid_pos_y_out;
	pid_type_def pid_pos_y_inner;
	pid_type_def pid_OutAngle;
	pid_type_def pid_InnerAngle;
	pid_type_def path_speed_pid;


	is_cpt_e POS_IS_CPT;                     
   
	
}ops_t;




int8_t Action_init(ops_t *o,const ops_param_t *param,Action_POS_t *pos);

int8_t go_path(ops_t *o,CMD_ACTION_t *ops_out);
#endif

